#include <cubEPrimitive.hh>
#include <cubEMath.hh>
#include<math.h>

namespace CubE
{
   const Vector3 Vector3::ZERO(0, 0, 0);
   const Vector3 Vector3::UNIT_X(1,0,0);
   const Vector3 Vector3::UNIT_Y(0,1,0);
   const Vector3 Vector3::UNIT_Z(0,0,1);
   
   Vector3::Vector3(void)
      :_x(0),
       _y(0),
       _z(0)
   {
      
   }
   Vector3::Vector3(float x, float y, float z)
      :_x(x),
       _y(y),
       _z(z)
   {
      
   }
   
   Vector3::Vector3(Vector3 const& vec)
      :_x(vec._x),
       _y(vec._y),
       _z(vec._z)
   {

   }
   
   Vector3::Vector3(Vector3& vec)
      :_x(vec._x),
       _y(vec._y),
       _z(vec._z)
   {

   }

   Vector3& Vector3::operator=(const Vector3& vec)
   {
      if(this==&vec) {
	 return *this;
      }

      _x = vec._x;
      _y = vec._y;
      _z = vec._z;

      return *this;
   }

   Vector3& Vector3::operator+=(const Vector3& vec)
   {
      *this = Vector3( _x + vec._x,
                       _y + vec._y,
                       _z + vec._z );
      return *this;
   }


   Vector3 Vector3::operator+(const Vector3& vec) const
   {
      return Vector3( _x + vec._x,
		      _y + vec._y,
		      _z + vec._z );
   }


   Vector3 Vector3::operator-(const Vector3& vec) const
   {
      return Vector3( _x - vec._x,
		      _y - vec._y,
		      _z - vec._z );
   }

   Vector3 Vector3::operator*(float factor) const
   {
      return Vector3 ( _x * factor,
		       _y * factor,
		       _z * factor );
   }
   
   float Vector3::operator[](int i) const
   {
      switch(i)
      {
      case 0:
	 return _x;

      case 1:
	 return _y;

      case 2:
	 return _z;

      default:
	 //Error Handling
	 return -99999.99f;
      }
   }

   bool Vector3::operator==(const Vector3& other) const
   {
      if(IsFloatEqual(_x, other._x) &&
	 IsFloatEqual(_y, other._y) &&
	 IsFloatEqual(_z, other._z) )
      {
	 return true;
      }
      
      return false;
   }

   bool Vector3::operator!=(const Vector3& other) const
   {
      if(*this==other) {
	 return false;
      }

      return true;
   }


   void Vector3::Get(float val[3])
   {
      if(val) {
	 val[0] = _x;
	 val[1] = _y;
	 val[2] = _z;
      }
   }

   float Vector3::Get(Component compo)
   {
      switch(compo) {
      case _X:
         return _x;

      case _Y:
         return _y;
         
      case _Z:
         return _z;

      default:
         //Error Handling
	 return -99999.99f;
      }
   }


   void Vector3::Set(float val[3])
   {
      if(val) {
	 _x = val[0];
	 _y = val[1];
	 _z = val[2];
      }
   }

   void Vector3::Set(float xval, float yval, float zval)
   {
      _x = xval;
      _y = yval;
      _z = zval;
   }

   float Vector3::Normalize(void)
   {
      float length = GetLength();
      if(IsFloatEqual(length, 0.0f)) {
	 return 0.0f;
      }
      
      _x = _x / length;
      _y = _y / length;
      _z = _z / length;
      
      return length;
   }

   bool Vector3::IsNormalized(void)
   {
      float length = GetLength();
      return IsFloatEqual(length, 1.0f);
   }

   float Vector3::GetLength(void)
   {
      return  sqrt( _x*_x + _y*_y + _z*_z);
   }

   Vector3 Vector3::CrossProduct(const Vector3& other) const
   {
      float rslt_x = _y*other._z - _z*other._y;
      float rslt_y = _z*other._x - _x*other._z;
      float rslt_z = _x*other._y - _y*other._x;

      return Vector3(rslt_x, rslt_y, rslt_z);
   }

   float Vector3::DotProduct(const Vector3& other) const
   {
      float result = _x*other._x + _y*other._y + _z*other._z;
      return result;
   }

   Vector3 Vector3::Rotate(const Quaternion& q) const
   {
      Quaternion qu = q;
      if(!qu.IsNormalized()) {
	 qu.Normalize();
      }
      
      Quaternion vec= Quaternion(*this, 0);
      Quaternion conj_qu = qu.GetConjugate();
      Vector3 ret = (qu * vec * conj_qu).GetImaginaryPart();
      
      return ret;
   }
}
